#pragma once
#include "Precompile.h"
#include "Vector.h"

namespace Utility{
	namespace Math{
		template <unsigned int Rows, unsigned int Collumns,typename InternalType>
		class Matrix;

		template<typename InternalType = float>
		class Quaternion{
		public:
			Quaternion(){
				identity();
			}

			Quaternion(InternalType *data){
				std::copy(data,data+4,vec.a);
			}

			Quaternion(InternalType x, InternalType y, InternalType z, InternalType w){
				vec(_x_,x);
				vec(_y_,y);
				vec(_z_,z);
				vec(_w_,w);
			}

			void createFromAxisAngle(Vector<InternalType> axis, InternalType angle){
				// Create from axis angle
				axis.normalize();
				InternalType sinAngle = InternalType(sin(angle/2.0));

				vec(_x_,axis(_x_)*sinAngle);
				vec(_y_,axis(_y_)*sinAngle);
				vec(_z_,axis(_z_)*sinAngle);
				vec(_w_,InternalType(cos(angle/2.0)));
			}

			void createFromMatrix(Matrix<4,4,InternalType>& matrix){
				float trace = matrix.get(0,0) + matrix.get(1,1) + matrix.get(2,2);
				if(trace > 0){
					float s = 0.5f/sqrtf(trace+1.0f);
					vec(_w_,0.25f/s);
					vec(_x_,(matrix.get(2,1)-matrix.get(1,2))*s);
					vec(_y_,(matrix.get(0,2)-matrix.get(2,0))*s);
					vec(_z_,(matrix.get(1,0)-matrix.get(0,1))*s);
				} 
				else {
					if ( matrix.get(0,0) > matrix.get(1,1) && matrix.get(0,0) > matrix.get(2,2) ) {
						float s = 2.0f * sqrtf( 1.0f + matrix.get(0,0) - matrix.get(1,1) - matrix.get(2,2));
						vec(_w_,(matrix.get(2,1)-matrix.get(1,2))/s);
						vec(_x_,0.25f *s);
						vec(_y_,(matrix.get(0,1)+matrix.get(1,0))/s);
						vec(_z_,(matrix.get(0,2)+matrix.get(2,0))/s);
					} 
					else if (matrix.get(1,1) > matrix.get(2,2)) {
						float s = 2.0f * sqrtf( 1.0f + matrix.get(1,1) - matrix.get(0,0) - matrix.get(2,2));
						vec(_w_,(matrix.get(0,2)-matrix.get(2,0))/s);
						vec(_x_,(matrix.get(0,1)+matrix.get(1,0))/s);
						vec(_y_,0.25f*s);
						vec(_z_,(matrix.get(1,2)+matrix.get(2,1))/s);
					} 
					else {
						float s = 2.0f * sqrtf( 1.0f + matrix.get(2,2) - matrix.get(0,0) - matrix.get(1,1) );
						vec(_w_,(matrix.get(1,0)-matrix.get(0,1))/s);
						vec(_x_,(matrix.get(0,2)+matrix.get(2,0))/s);
						vec(_y_,(matrix.get(1,2)+matrix.get(2,1))/s);
						vec(_z_,0.25f*s);
					}
				}
			}

			void identity(){
				vec(_x_,InternalType(0.0));
				vec(_y_,InternalType(0.0));
				vec(_z_,InternalType(0.0));
				vec(_w_,InternalType(1.0));
			}

			InternalType length(){
				return sqrt(vec(_x_)*vec(_x_) + vec(_y_)*vec(_y_) + vec(_z_)*vec(_z_) + vec(_w_)*vec(_w_));
			}

			void normalize(){
				InternalType len = length();
				vec /= len;
			}

			Quaternion<InternalType> conjugate(){
				Quaternion<InternalType> result = *this;
				result.vec(_x_,-result.vec(_x_));
				result.vec(_y_,-result.vec(_y_));
				result.vec(_z_,-result.vec(_z_));
				return result;
			}

			Quaternion<InternalType> slerp(Quaternion<InternalType> &other, float t) {
				Quaternion<InternalType> qm;
				
				// Calculate angle between them.
				InternalType cosHalfTheta =	vec(Utility::Math::_w_)*other.vec(Utility::Math::_w_) + 
											vec(Utility::Math::_x_)*other.vec(Utility::Math::_x_) + 
											vec(Utility::Math::_y_)*other.vec(Utility::Math::_y_) + 
											vec(Utility::Math::_z_)*other.vec(Utility::Math::_w_);

				// if qa=qb or qa=-qb then theta = 0 and we can return qa
				if(abs(cosHalfTheta) >= 1.0){
					return *this;
				}

				// Calculate temporary values.
				InternalType halfTheta = acos(cosHalfTheta);
				InternalType sinHalfTheta = InternalType(sqrt(1.0 - cosHalfTheta*cosHalfTheta));

				// If theta = 180 degrees then result is not fully defined
				// we could rotate around any axis normal to qa or qb
				if(fabs(sinHalfTheta) < 0.001){
					qm.vec(Utility::Math::_w_,(vec(Utility::Math::_w_)*InternalType(0.5) + other.vec(Utility::Math::_w_)*InternalType(0.5)));
					qm.vec(Utility::Math::_x_,(vec(Utility::Math::_x_)*InternalType(0.5) + other.vec(Utility::Math::_x_)*InternalType(0.5)));
					qm.vec(Utility::Math::_y_,(vec(Utility::Math::_y_)*InternalType(0.5) + other.vec(Utility::Math::_y_)*InternalType(0.5)));
					qm.vec(Utility::Math::_z_,(vec(Utility::Math::_z_)*InternalType(0.5) + other.vec(Utility::Math::_z_)*InternalType(0.5)));
					return qm;
				}
				InternalType ratioA = sin((1-t)*halfTheta)/sinHalfTheta;
				InternalType ratioB = sin(t*halfTheta)/sinHalfTheta;

				// Calculate Quaternion.
				qm.vec(Utility::Math::_w_,(vec(Utility::Math::_w_)*ratioA + other.vec(Utility::Math::_w_)*ratioB));
				qm.vec(Utility::Math::_x_,(vec(Utility::Math::_x_)*ratioA + other.vec(Utility::Math::_x_)*ratioB));
				qm.vec(Utility::Math::_y_,(vec(Utility::Math::_y_)*ratioA + other.vec(Utility::Math::_y_)*ratioB));
				qm.vec(Utility::Math::_z_,(vec(Utility::Math::_z_)*ratioA + other.vec(Utility::Math::_z_)*ratioB));
				return qm;
			}


			/// Overloads
			// Multiplication overloads
			void operator*= (Quaternion &other){
				vec(_x_, vec(_w_)*other.vec(_x_) + vec(_x_)*other.vec(_w_) + vec(_y_)*other.vec(_z_) - vec(_z_)*other.vec(_y_));
				vec(_y_, vec(_w_)*other.vec(_y_) + vec(_y_)*other.vec(_w_) + vec(_z_)*other.vec(_x_) - vec(_x_)*other.vec(_z_));
				vec(_z_, vec(_w_)*other.vec(_z_) + vec(_z_)*other.vec(_w_) + vec(_x_)*other.vec(_y_) - vec(_y_)*other.vec(_x_));
				vec(_w_, vec(_w_)*other.vec(_w_) - vec(_x_)*other.vec(_x_) - vec(_y_)*other.vec(_y_) - vec(_z_)*other.vec(_z_));
			}

			Quaternion operator* (Quaternion &other){
				Quaternion result;

				result.vec(_x_, vec(_w_)*other.vec(_x_) + vec(_x_)*other.vec(_w_) + vec(_y_)*other.vec(_z_) - vec(_z_)*other.vec(_y_));
				result.vec(_y_, vec(_w_)*other.vec(_y_) + vec(_y_)*other.vec(_w_) + vec(_z_)*other.vec(_x_) - vec(_x_)*other.vec(_z_));
				result.vec(_z_, vec(_w_)*other.vec(_z_) + vec(_z_)*other.vec(_w_) + vec(_x_)*other.vec(_y_) - vec(_y_)*other.vec(_x_));
				result.vec(_w_, vec(_w_)*other.vec(_w_) - vec(_x_)*other.vec(_x_) - vec(_y_)*other.vec(_y_) - vec(_z_)*other.vec(_z_));

				return result;
			}

			Vector<InternalType,4> vec;
		};
	}
}